﻿using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using Unity.Mathematics;
using Unity.Collections;
using Unity.Jobs;
using Unity.Burst;

public class Pathfinding : MonoBehaviour
{
    private const int MOVE_STRAIGHT_COST = 10;

    public const int size = 20;
    public Vector2Int start;
    public Vector2Int end;

    NativeArray<PathNode> m_pathNodeArray;
    int2 m_gridSize;


    private void Start()
    {
        m_gridSize = new int2(size, size);
        m_pathNodeArray = new NativeArray<PathNode>(m_gridSize.x * m_gridSize.y, Allocator.Persistent);
    }

    private void OnDestroy()
    {
        m_pathNodeArray.Dispose();
    }


    private void Update()
    {
        /*FindPath(new int2(1, 2), new int2(98, 56));*/


        PathFindingJob job = new PathFindingJob()
        {
            startPosition = new int2(start.x, start.y),
            endPosition = new int2(end.x, end.y),
            pathNodeArray = m_pathNodeArray,
            gridSize = m_gridSize
        };
        JobHandle jbh = job.Schedule();
        jbh.Complete();
    }

    [BurstCompile]
    public struct PathFindingJob : IJob
    {
        public int2 startPosition;
        public int2 endPosition;
        public NativeArray<PathNode> pathNodeArray;
        public int2 gridSize;
        public void Execute()
        {
            for (int x = 0; x < gridSize.x; x++)
            {
                for (int y = 0; y < gridSize.y; y++)
                {
                    PathNode pathNode = new PathNode();
                    pathNode.x = x;
                    pathNode.y = y;
                    pathNode.index = CalculateIndex(x, y, gridSize.x);

                    pathNode.gCost = int.MaxValue;
                    pathNode.hCost = CalculateDistanceCost(new int2(x, y), endPosition);
                    pathNode.CalculateFCost();

                    pathNode.isWalkable = true;
                    pathNode.cameFromNodeIndex = -1;

                    pathNodeArray[pathNode.index] = pathNode;
                }
            }

            NativeArray<int2> neighbourOffsetArray = new NativeArray<int2>(4, Allocator.Temp);
            neighbourOffsetArray[0] = new int2(-1, 0);
            neighbourOffsetArray[1] = new int2(+1, 0);
            neighbourOffsetArray[2] = new int2(0, -1);
            neighbourOffsetArray[3] = new int2(0, +1);

            int endNodeIndex = CalculateIndex(endPosition.x, endPosition.y, gridSize.x);

            PathNode startNode = pathNodeArray[CalculateIndex(startPosition.x, startPosition.y, gridSize.x)];
            startNode.gCost = 0;
            startNode.CalculateFCost();
            pathNodeArray[startNode.index] = startNode;

            NativeList<int> openList = new NativeList<int>(Allocator.Temp);
            NativeList<int> closedList = new NativeList<int>(Allocator.Temp);

            openList.Add(startNode.index);

            while (openList.Length > 0)
            {
                int currentNodeIndex = GetLowestFCostNodeIndex(openList, pathNodeArray);
                PathNode currentNode = pathNodeArray[currentNodeIndex];

                if (currentNodeIndex == endNodeIndex)
                {
                    //Reached
                    break;
                }

                //Remove current Node from open list
                for (int i = 0; i < openList.Length; i++)
                {
                    if (openList[i] == currentNodeIndex)
                    {
                        openList.RemoveAtSwapBack(i);
                        break;
                    }
                }

                closedList.Add(currentNodeIndex);

                for (int i = 0; i < neighbourOffsetArray.Length; i++)
                {
                    int2 neighbourOffset = neighbourOffsetArray[i];
                    int2 neighbourPosition = new int2(currentNode.x + neighbourOffset.x, currentNode.y + neighbourOffset.y);

                    if (!IsPositioninsideGrid(neighbourPosition, gridSize))
                        continue;

                    int neighbourNodeIndex = CalculateIndex(neighbourPosition.x, neighbourPosition.y, gridSize.x);

                    if (closedList.Contains(neighbourNodeIndex))
                        continue;

                    PathNode neighbourNode = pathNodeArray[neighbourNodeIndex];
                    if (!neighbourNode.isWalkable)
                        continue;

                    int2 currentNodePosition = new int2(currentNode.x, currentNode.x);

                    int newMovementCostToNeighbour = 0;
                    if (IsTurnNode(currentNodeIndex, neighbourNodeIndex, pathNodeArray))
                        newMovementCostToNeighbour = currentNode.gCost + CalculateDistanceCost(currentNodePosition, neighbourPosition) + 1;
                    else
                        newMovementCostToNeighbour = currentNode.gCost + CalculateDistanceCost(currentNodePosition, neighbourPosition);

                    if (newMovementCostToNeighbour < neighbourNode.gCost)
                    {
                        neighbourNode.gCost = newMovementCostToNeighbour;
                        neighbourNode.cameFromNodeIndex = currentNodeIndex;
                        neighbourNode.CalculateFCost();
                        pathNodeArray[neighbourNodeIndex] = neighbourNode;

                        if (!openList.Contains(neighbourNode.index))
                            openList.Add(neighbourNode.index);


                    }
                }

            }

            PathNode endNode = pathNodeArray[endNodeIndex];
            if (endNode.cameFromNodeIndex == -1)
            {
                //no path found
                //print("no path found");
            }
            else
            {
                //path found
                //print("path found");
                NativeList<int2> path = CalculatePath(pathNodeArray, endNode);
                /*foreach (int2 pathPosition in path)
                {
                    print(pathPosition);
                }*/
                path.Dispose();
            }

            neighbourOffsetArray.Dispose();
            /*openList.Dispose();*/
            closedList.Dispose();
            /*pathNodeArray.Dispose();*/
        }

        

        private NativeList<int2> CalculatePath(NativeArray<PathNode> pathNodeArray, PathNode endNode)
        {
            if (endNode.cameFromNodeIndex == -1)
            {
                return new NativeList<int2>(Allocator.Temp);
            }
            else
            {
                NativeList<int2> path = new NativeList<int2>(Allocator.Temp);
                path.Add(new int2(endNode.x, endNode.y));

                PathNode currentNode = endNode;
                while (currentNode.cameFromNodeIndex != -1)
                {
                    PathNode cameFromNode = pathNodeArray[currentNode.cameFromNodeIndex];
                    path.Add(new int2(cameFromNode.x, cameFromNode.y));
                    currentNode = cameFromNode;
                }
                return path;
            }
        }

        private bool IsPositioninsideGrid(int2 gridPosition, int2 gridSize)
        {
            return
                gridPosition.x >= 0 &&
                gridPosition.y >= 0 &&
                gridPosition.x < gridSize.x &&
                gridPosition.y < gridSize.y;
        }

        private int CalculateIndex(int x, int y, int gridWidth)
        {
            return gridWidth * x + y;
        }

        private int CalculateDistanceCost(int2 aPosition, int2 bPosition)
        {
            return (math.abs(aPosition.x - bPosition.x) + math.abs(aPosition.y - bPosition.y)) * MOVE_STRAIGHT_COST;
        }

        private int GetLowestFCostNodeIndex(NativeArray<int> openList, NativeArray<PathNode> pathNodeArray)
        {
            PathNode lowestCostPathNode = pathNodeArray[openList[0]];
            for (int i = 1; i < openList.Length; i++)
            {
                PathNode testPathNode = pathNodeArray[openList[i]];
                if (testPathNode.fCost < lowestCostPathNode.fCost)
                {
                    lowestCostPathNode = testPathNode;
                }
            }
            return lowestCostPathNode.index;
        }

        public bool IsTurnNode(int currentNodeIndex, int neighbourIndex, NativeArray<PathNode> pathNodeArray)
        {
            int cameFromNodeIndex = pathNodeArray[currentNodeIndex].cameFromNodeIndex;
            if (cameFromNodeIndex != -1)
            {
                int2 cameFromNodePosition = new int2(pathNodeArray[cameFromNodeIndex].x, pathNodeArray[cameFromNodeIndex].y);
                int2 currentNodePosition = new int2(pathNodeArray[currentNodeIndex].x, pathNodeArray[currentNodeIndex].y);
                int2 neighbourNodePosition = new int2(pathNodeArray[neighbourIndex].x, pathNodeArray[neighbourIndex].y);

                bool b = (currentNodePosition.x - cameFromNodePosition.x) == (neighbourNodePosition.x - currentNodePosition.x);
                return !b;
            }
            return false;
        }
    }


    private void FindPath(int2 startPosition, int2 endPosition)
    {
        int2 gridSize = new int2(128, 128);

        NativeArray<PathNode> pathNodeArray = new NativeArray<PathNode>(gridSize.x * gridSize.y, Allocator.Temp);

        for (int x = 0; x < gridSize.x; x++)
        {
            for (int y = 0; y < gridSize.y; y++)
            {
                PathNode pathNode = new PathNode();
                pathNode.x = x;
                pathNode.y = y;
                pathNode.index = CalculateIndex(x, y, gridSize.x);

                pathNode.gCost = int.MaxValue;
                pathNode.hCost = CalculateDistanceCost(new int2(x, y), endPosition);
                pathNode.CalculateFCost();

                pathNode.isWalkable = true;
                pathNode.cameFromNodeIndex = -1;

                pathNodeArray[pathNode.index] = pathNode;
            }
        }

        NativeArray<int2> neighbourOffsetArray = new NativeArray<int2>(new int2[]
        {
            new int2(-1, 0),    //left
            new int2(+1, 0),    //right
            new int2(0, -1),    //Down
            new int2(0, +1)     //Up
        }, Allocator.Temp);

        int endNodeIndex = CalculateIndex(endPosition.x, endPosition.y, gridSize.x);

        PathNode startNode = pathNodeArray[CalculateIndex(startPosition.x, startPosition.y, gridSize.x)];
        startNode.gCost = 0;
        startNode.CalculateFCost();
        pathNodeArray[startNode.index] = startNode;

        NativeList<int> openList = new NativeList<int>(Allocator.Temp);
        NativeList<int> closedList = new NativeList<int>(Allocator.Temp);

        openList.Add(startNode.index);

        while(openList.Length > 0)
        {
            int currentNodeIndex = GetLowestFCostNodeIndex(openList, pathNodeArray);
            PathNode currentNode = pathNodeArray[currentNodeIndex];

            if(currentNodeIndex == endNodeIndex)
            {
                //Reached
                break;
            }

            //Remove current Node from open list
            for (int i = 0; i < openList.Length; i++)
            {
                if(openList[i]==currentNodeIndex)
                {
                    openList.RemoveAtSwapBack(i);
                    break;
                }
            }

            closedList.Add(currentNodeIndex);

            for (int i = 0; i < neighbourOffsetArray.Length; i++)
            {
                int2 neighbourOffset = neighbourOffsetArray[i];
                int2 neighbourPosition = new int2(currentNode.x + neighbourOffset.x, currentNode.y + neighbourOffset.y);

                if (!IsPositioninsideGrid(neighbourPosition, gridSize))
                    continue;

                int neighbourNodeIndex = CalculateIndex(neighbourPosition.x, neighbourPosition.y, gridSize.x);

                if (closedList.Contains(neighbourNodeIndex))
                    continue;

                PathNode neighbourNode = pathNodeArray[neighbourNodeIndex];
                if (!neighbourNode.isWalkable)
                    continue;

                int2 currentNodePosition = new int2(currentNode.x, currentNode.x);

                int newMovementCostToNeighbour = 0;
                if (IsTurnNode(currentNodeIndex, neighbourNodeIndex, pathNodeArray))
                    newMovementCostToNeighbour = currentNode.gCost + CalculateDistanceCost(currentNodePosition, neighbourPosition) + 1;
                else
                    newMovementCostToNeighbour = currentNode.gCost + CalculateDistanceCost(currentNodePosition, neighbourPosition);

                if (newMovementCostToNeighbour < neighbourNode.gCost)
                {
                    neighbourNode.gCost = newMovementCostToNeighbour;
                    neighbourNode.cameFromNodeIndex = currentNodeIndex;
                    neighbourNode.CalculateFCost();
                    pathNodeArray[neighbourNodeIndex] = neighbourNode;

                    if (!openList.Contains(neighbourNode.index))
                        openList.Add(neighbourNode.index);
                }
            }

        }

        PathNode endNode = pathNodeArray[endNodeIndex];
        if(endNode.cameFromNodeIndex == -1)
        {
            //no path found
            print("no path found");
        }
        else
        {
            //path found
            NativeList<int2> path = CalculatePath(pathNodeArray, endNode);
            /*foreach (int2 pathPosition in path)
            {
                print(pathPosition);
            }*/
            path.Dispose();
        }

        neighbourOffsetArray.Dispose();
        openList.Dispose();
        closedList.Dispose();
        pathNodeArray.Dispose();
    }

    private NativeList<int2> CalculatePath(NativeArray<PathNode> pathNodeArray, PathNode endNode)
    {
        if(endNode.cameFromNodeIndex == -1)
        {
            return new NativeList<int2>(Allocator.Temp);
        }
        else
        {
            NativeList<int2> path = new NativeList<int2>(Allocator.Temp);
            path.Add(new int2(endNode.x, endNode.y));

            PathNode currentNode = endNode;
            while (currentNode.cameFromNodeIndex != -1)
            {
                PathNode cameFromNode = pathNodeArray[currentNode.cameFromNodeIndex];
                path.Add(new int2(cameFromNode.x, cameFromNode.y));
                currentNode = cameFromNode;
            }
            return path;
        }
    }


    private bool IsPositioninsideGrid(int2 gridPosition, int2 gridSize)
    {
        return
            gridPosition.x >= 0 &&
            gridPosition.y >= 0 &&
            gridPosition.x < gridSize.x &&
            gridPosition.y < gridSize.y;
    }

    private int CalculateIndex(int x, int y, int gridWidth)
    {
        return gridWidth * x + y;
    }

    private int CalculateDistanceCost(int2 aPosition, int2 bPosition)
    {
        return (math.abs(aPosition.x - bPosition.x) + math.abs(aPosition.y - bPosition.y)) * MOVE_STRAIGHT_COST;
    }

    private int GetLowestFCostNodeIndex(NativeArray<int> openList, NativeArray<PathNode> pathNodeArray)
    {
        PathNode lowestCostPathNode = pathNodeArray[openList[0]];
        for (int i = 1; i < openList.Length; i++)
        {
            PathNode testPathNode = pathNodeArray[openList[i]];
            if(testPathNode.fCost < lowestCostPathNode.fCost)
            {
                lowestCostPathNode = testPathNode;
            }
        }
        return lowestCostPathNode.index;
    }

    public bool IsTurnNode(int currentNodeIndex, int neighbourIndex, NativeArray<PathNode> pathNodeArray)
    {
        int cameFromNodeIndex = pathNodeArray[currentNodeIndex].cameFromNodeIndex;
        if (cameFromNodeIndex != -1)
        {
            int2 cameFromNodePosition = new int2(pathNodeArray[cameFromNodeIndex].x, pathNodeArray[cameFromNodeIndex].y);
            int2 currentNodePosition = new int2(pathNodeArray[currentNodeIndex].x, pathNodeArray[currentNodeIndex].y);
            int2 neighbourNodePosition = new int2(pathNodeArray[neighbourIndex].x, pathNodeArray[neighbourIndex].y);

            bool b = (currentNodePosition.x - cameFromNodePosition.x) == (neighbourNodePosition.x - currentNodePosition.x);
            return !b;
        }
        return false;
    }
}
